#ifndef MAINWINDOW_H
#define MAINWINDOW_H

#include <QMainWindow>
#include <qtimer.h>
#include <qeventloop.h>
#include <Eigen/Eigen>
#include <vector>
#include "TCPIPRobot.hpp"
#include "optoforce_data.h"
#include "single_innfos_joint.h"
#include "AeroRobotControl.h"
#include "UtilesIO.h"
#include <fstream>

#define RightGraspControl 8
#define LeftGraspControl 14
namespace Ui {
class MainWindow;
}

class MainWindow : public QMainWindow
{
    Q_OBJECT

public:
    explicit MainWindow(QWidget *parent = 0);
    ~MainWindow();

public slots:
    void force_data_loop_run();

    void seven_axis_data_loop_run();

    void drag_loop_run();

    void usleep_without_block(int us);

    void arm_task_move_z(double distance, double velocity);

private slots:
    void on_btn_GraspOpen_clicked();

    void on_btn_GraspClose_clicked();

    void on_btn_data_start_clicked();

    void on_btn_data_stop_clicked();

    void on_btn_GraspConnect_clicked();

    void on_force_range_valueChanged(int arg1);

    void on_torque_range_valueChanged(int arg1);

    void on_btn_seven_axis_on_clicked();

    void on_btn_seven_axis_off_clicked();

    void on_btn_seven_axis_zero_clicked();

    void on_seven_axis_rotate_start_clicked();

    void on_seven_axis_rotate_stop_clicked();

    void on_seven_axis_velocity_set_valueChanged(int arg1);

    void on_seven_axis_absolute_position_valueChanged(double arg1);

    void on_btn_seven_axis_to_absolute_pos_clicked();

    void on_seven_axis_velocity_set_with_time_valueChanged(int arg1);

    void on_seven_axis_rotate_time_valueChanged(int arg1);

    void on_seven_axis_velocity_set_with_current_valueChanged(int arg1);

    void on_seven_axis_current_limit_valueChanged(double arg1);

    void on_seven_axis_rotate_start_with_time_clicked();

    void on_seven_axis_rotate_start_with_current_clicked();

    void on_seven_axis_jointid_valueChanged(int arg1);

    void on_seven_axis_position_bar_valueChanged(int value);

    void on_seven_axis_position_bar_sliderPressed();

    void on_seven_axis_position_bar_sliderReleased();

    void on_drag_on_button_clicked(bool checked);

    void on_drag_on_button_toggled(bool checked);

    void on_drag_off_button_clicked();

    void on_force_limit_valueChanged(double arg1);

    void on_force_ratio_valueChanged(double arg1);

    void on_force_sensor_zero_button_clicked();

    void on_force_sensor_unzero_button_clicked();

    void on_arm_on_button_clicked();

    void on_adjust_step_valueChanged(double arg1);

    void on_step_length_valueChanged(double arg1);

    void on_in_button_clicked();

    void on_out_button_clicked();

    void on_out_button_pressed();

    void on_in_continues_button_clicked();

    void on_screw_in_button_pressed();

    void on_screw_to_home_button_clicked();

    void on_screw_in_button_clicked();

    void on_screw_out_button_pressed();

    void on_screw_out_button_clicked();

    void on_in_button_pressed();

    void on_rotate_one_step_clicked();

    void on_button_2_clicked();

private:
    Ui::MainWindow *ui;
    int force_range = 50;
    int torque_range = 9;

    TCPIPRobot *_tcpipGrasp=nullptr;
    TCPIPRobot *_tcpipLeftGrasp=nullptr;
    int isGraspConnect = -1;

    QTimer *force_data_loop;
    optoforce_data * force_data_receiver = nullptr;
    bool is_data_receiving = false;
    double force_data[6] = {0, 0, 0, 0, 0, 0};
    double force_data_buffer[6] = {0, 0, 0, 0, 0, 0};
    double force_data_buffer_2[6] = {0, 0, 0, 0, 0, 0};
    double force_data_buffer_3[6] = {0, 0, 0, 0, 0, 0};
    char force_sensor_ip[14] = "192.168.1.105";

    QTimer *seven_axis_data_loop;
    single_innfos_joint * seven_axis_control = nullptr;
    bool is_seven_axis_on = false;
    double * seven_axis_position = nullptr;
    double * seven_axis_velocity = nullptr;
    double * seven_axis_current = nullptr;
    double seven_axis_velocity_set = 1200;
    double seven_axis_velocity_set_with_time = 1200;
    double seven_axis_velocity_set_with_current = 1200;
    int seven_axis_rotate_time = 3;
    double seven_axis_rotate_current_limet = 1;
    double seven_axis_position_set = -1.2;
    int seven_axis_jointid = 7;
    bool is_bar_moved_by_user = false;
    double seven_axis_position_set_by_bar = 0;

    QTimer *drag_loop;
    AeroRobot::AeroRobotControl * arm_control = nullptr;
    bool is_drag_on = false;
    double arm_current_pose[6];
    double arm_joint_position[6];
    double joint_6_home_position = -1.95512;
    double arm_target_pose[8] = {0, 0, 0, 0, 0, 0, 0, 0};
    double endpoint_adjust[3] = {0, 0, 0};
    Eigen::Matrix4d arm_end_trans_matrix = Eigen::Matrix4d::Identity();
    Eigen::Matrix3d arm_end_rotate_matrix = Eigen::Matrix3d::Identity();
    Eigen::Vector3d end_coordinate = Eigen::Vector3d::Zero();
    Eigen::Vector3d base_coordinate = Eigen::Vector3d::Zero();
    double force_limit = 15;
    double force_ratio = 0.01;
    double adjust_step = 0.001;
    double step_length = 0.002;
    double adjust_force_limit = 3;

    QTimer *usleep_no_block;
    QEventLoop *usleep_loop;

    bool is_out_button_pressed = false;
    bool is_in_button_pressed = false;
    bool is_screw_in_button_pressed = false;
    bool is_screw_out_button_pressed = false;

    //grasp control
    const unsigned char buffGraspInit[RightGraspControl] = {0x01, 0x06, 0x01, 0x00, 0x00, 0x01, 0x49, 0xF6};
    const unsigned char buffGraspFullInit[RightGraspControl] = {0x01, 0x06, 0x01, 0x00, 0x00, 0xA5, 0x48, 0x4D};
    const unsigned char buffGraspOpen[RightGraspControl] = {0x01, 0x06, 0x01, 0x03, 0x03, 0xE8, 0x78, 0x88};
    const unsigned char buffGraspClose[RightGraspControl] = {0x01, 0x06, 0x01, 0x03, 0x00, 0x00, 0x78, 0x36};

    //left grasp control
    const unsigned char buffLeftGraspInit[LeftGraspControl] = {0xFF, 0xFE, 0xFD, 0xFC, 0x01, 0x08, 0x02, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFB};//init
    const unsigned char buffLeftGraspOpen[LeftGraspControl] = {0xFF, 0xFE, 0xFD, 0xFC, 0x01, 0x06, 0x02, 0x01, 0x00, 0x64, 0x00, 0x00, 0x00, 0xFB};
    const unsigned char buffLeftGraspClose[LeftGraspControl] = {0xFF, 0xFE, 0xFD, 0xFC, 0x01, 0x06, 0x02, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFB};
};

#endif // MAINWINDOW_H
